function [OutPutLineY0_f,OutPutLineAlpPsi_f,OutPutLineKapC0_f,OutPutLineKapDxC1_f] = l_CalcLineFactorWithCubicPolynomial_V(...
    LineViewEnd_f,...
    LineQualityK1_f,...
    LineDy0K1_f,...
    LineAlpPsiK1_f,...
    LineKapC0K1_f,...
    LineKapDxC1K1_f,...
    LineDy0_f,...
    LineAlpPsi_f,...
    LinekapC0_f,...
    LinekapDxC1_f,...
    LineType_ub,...
    t_VehSpd_f,...
    t_EgoYawrate_f)


% FLOAT  LineViewEnd_f,
% FLOAT  LineQualityK1_f,
% FLOAT  LineDy0K1_f,
% FLOAT  LineAlpPsiK1_f,
% FLOAT  LineKapC0K1_f,
% FLOAT  LineKapDxC1K1_f,
% FLOAT  LineDy0_f,
% FLOAT  LineAlpPsi_f,
% FLOAT  LinekapC0_f,
% FLOAT  LinekapDxC1_f,
% UB     LineType_ub,
% FLOAT& OutPutLineY0_f,
% FLOAT& OutPutLineAlpPsi_f,
% FLOAT& OutPutLineKapC0_f,
% FLOAT& OutPutLineKapDxC1_f



% {
%     const FLOAT t_VehSpd_f             = VDB_GetEviVehicleSpeed_F();
%     FLOAT       t_PreDist_f            = 0;
%     FLOAT       t_LongDist_f[4]        = {0.0f};
%     FLOAT       t_y[4]                 = {0.0f};
%     FLOAT       t_cubicPolynomial_f[4] = {0.0f};
%     FLOAT       t_DevAngle_f           = 0;
%     FLOAT       t_DevDist_f            = 0;

t_LongDist_f = zeros(1,4,'single');
t_y = zeros(1,4,'single');
t_cubicPolynomial_f = zeros(1,4,'single');


if LineQualityK1_f > 0

    t_PreDist_f     = min(max((t_VehSpd_f * 1.8), 15.0), LineViewEnd_f);
    t_LongDist_f(1) = 0.05 * t_PreDist_f;
    t_LongDist_f(2) = 0.3 * t_PreDist_f;
    t_LongDist_f(3) = 0.6 * t_PreDist_f;
    t_LongDist_f(4) = t_PreDist_f;
    for i = 1:4

        t_y(i) = CalcDClothoidApproximation_f(t_LongDist_f(i), LineDy0K1_f, LineAlpPsiK1_f, LineKapC0K1_f, LineKapDxC1K1_f);
    end

    t_CycleTime_f = single(0.02);
    [m_SinOfPsi_f,m_CosOfPsi_f,m_DeltaX_f,m_DeltaY_f] =  l_CalcEgoVehicleMovement_V(t_EgoYawrate_f,t_VehSpd_f,t_CycleTime_f);

    for i = 1:4
        t_LongDist_f(i) = t_LongDist_f(i) - m_DeltaX_f;
        t_y(i)          = t_y(i) - m_DeltaY_f;
        t_LongDist_f(i) = t_LongDist_f(i) * m_CosOfPsi_f + t_y(i) * m_SinOfPsi_f;
        t_y(i)          = -t_LongDist_f(i) * m_SinOfPsi_f + t_y(i) * m_CosOfPsi_f;
    end
    % l_CalcCubicPolynomial_V(t_LongDist_f, t_y, t_cubicPolynomial_f);
    t_cubicPolynomial_f = polyfit(t_LongDist_f,t_y,3);  %Not from imotion,use matlab function
    t_cubicPolynomial_f = flip(t_cubicPolynomial_f);

    t_DevAngle_f = 57.3 * (LineAlpPsi_f - t_cubicPolynomial_f(1));
    t_DevDist_f  = (LineDy0_f - LineDy0K1_f) / sqrt(LineAlpPsi_f * LineAlpPsi_f + 1);

    t_Y0Diff_f = single(0);
    if abs(t_DevAngle_f) < 5 && abs(t_DevDist_f) < 1
        t_Y0Diff_f = LineDy0_f - t_cubicPolynomial_f(0);


        %%
        % SEM.IF_PCP_SEM_LANE_TYPE = struct;
        % SEM.IF_PCP_SEM_LANE_TYPE.IF_PCP_SEM_LINE_UNDECIDED = 0;
        % SEM.IF_PCP_SEM_LANE_TYPE.IF_PCP_SEM_SOLID_LINE = 1;
        % SEM.IF_PCP_SEM_LANE_TYPE.IF_PCP_SEM_DASH_LINE = 2;
        % SEM.IF_PCP_SEM_LANE_TYPE.IF_PCP_SEM_DML_LINE = 3;
        % SEM.IF_PCP_SEM_LANE_TYPE.IF_PCP_SEM_BOTTS_DOTS = 4;
        % SEM.IF_PCP_SEM_LANE_TYPE.IF_PCP_SEM_DECELERATION = 5;
        % SEM.IF_PCP_SEM_LANE_TYPE.IF_PCP_SEM_HOV_LANE = 6;
        % SEM.IF_PCP_SEM_LANE_TYPE.IF_PCP_SEM_ROADEDGE = 7;
        if (abs(LinekapC0_f) < 0.0005 && IF_PCP_SEM_DML_LINE == LineType_ub && IF_PCP_SEM_DECELERATION == LineType_ub)
            t_Y0Diff_f = min(t_Y0Diff_f, 0.05);
        end  % IF无法满足，死逻辑
        %%


        if abs(LinekapC0_f) < 0.002
            % PAG_GetoriginalLineFilterFactorPAG_F = 0.3
            OutPutLineY0_f     = PAG_GetoriginalLineFilterFactorPAG_F() * (t_Y0Diff_f) + t_cubicPolynomial_f(1);
            OutPutLineAlpPsi_f = PAG_GetoriginalLineFilterFactorPAG_F() * (LineAlpPsi_f - t_cubicPolynomial_f(2)) + t_cubicPolynomial_f(2);
            OutPutLineKapC0_f = PAG_GetoriginalLineFilterFactorPAG_F() * (LinekapC0_f - t_cubicPolynomial_f(3)) + t_cubicPolynomial_f(3);
            OutPutLineKapDxC1_f = PAG_GetoriginalLineFilterFactorPAG_F() * (LinekapDxC1_f - t_cubicPolynomial_f(4)) + t_cubicPolynomial_f(4);
        else
            % PAG_GetLineFilterCurveFactorPAG_F = 0.8
            OutPutLineY0_f     = PAG_GetLineFilterCurveFactorPAG_F() * (t_Y0Diff_f) + t_cubicPolynomial_f(1);
            OutPutLineAlpPsi_f = PAG_GetLineFilterCurveFactorPAG_F() * (LineAlpPsi_f - t_cubicPolynomial_f(2)) + t_cubicPolynomial_f(2);
            OutPutLineKapC0_f = PAG_GetLineFilterCurveFactorPAG_F() * (LinekapC0_f - t_cubicPolynomial_f(3))+ t_cubicPolynomial_f(3);
            OutPutLineKapDxC1_f = PAG_GetLineFilterCurveFactorPAG_F() * (LinekapDxC1_f - t_cubicPolynomial_f(4)) + t_cubicPolynomial_f(4);

        end
    end
end
end

function z = PAG_GetLineFilterCurveFactorPAG_F()
z = single(0.8);
end

function z = PAG_GetoriginalLineFilterFactorPAG_F()
z = single(0.3);
end
